#! /usr/bin/env python
# -*- coding:UTF-8 -*-
import cv2
import numpy as np

# 寻找蓝线
import get_blue_line as blue_line
# 巡线基础控制模块
import wig_lane_dec
# 图像处理模块


''' 巡线'''
left_first_bianjie_x = 0  # 左边边界
right_first_bianjie_x = 0  # 右边边界
left_second_bianjie_x = 0
right_second_bianjie_x = 0
xunxian_first_shibie_xian = 430  # 原始是460
xunxian_second_shibie_xian = 430
back_shibie_xian = 460
left_shu_score = 0
right_shu_score = 0
error_last = 0
error_sum = [0, 0, 0, 0, 0]
error_sum_index = 0
mid_point = 320
detect_line1 = 230
detect_line2 = 210
'''蓝线'''
stop = 0
blue_line_heng_detect = 375  # y=350
blue_line_shu_detect = 320  # x=320
speed_dianji_fuhao = -1
jiaozhun_angle_sum = [0, 0, 0, 0, 0]
jiaozhun_shuju_ave_index = 0
angle_error_last = 0
blue_lane_score = 0

# 过去的端点
p1_last = (0, 0)
p2_last = (0, 0)


# 进行巡线操作的相关函数
def xunxian_final(img):
    # 对图片进行处理

    warp_img = wig_lane_dec.warp_image(img)
    bina_img = wig_lane_dec.Binary_image(warp_img)

    left_bound_445, right_bound_445 = wig_lane_dec.boundary_detect(bina_img)

    dec_line_mode = wig_lane_dec.get_line_mode(left_bound_445, right_bound_445)

    angle, pos_flag = wig_lane_dec.get_angle(left_bound_445, right_bound_445, dec_line_mode, bina_img)

    print('angle_original:', angle)

    # 角度的固定调0
    steering_angle = angle - 6

    print('angle:', steering_angle)

    # -------------------------------------------------------------------
    cv2.imshow('bina_img', bina_img)

    # 等待10ms看有没有按键按下，读取指令
    key_wig = cv2.waitKey(1)
    # ---------------------------------------------------------------------

    return steering_angle, pos_flag


# 蓝线检测(暂时没有使用，更换为wig的蓝线检测了)
def find_blue_line_final(img):
        global stop
        global blue_lane_score
        stop = 0
        blue_lower_line = np.array([100, 110, 50])
        blue_upper_line = np.array([129, 250, 255])
        new_img = img
        hsv_img = cv2.cvtColor(new_img, cv2.COLOR_BGR2HSV)

        blue_mask = cv2.inRange(hsv_img, blue_lower_line, blue_upper_line)

        ret, binary = cv2.threshold(blue_mask, 0, 255, cv2.THRESH_BINARY)

        kernel = np.ones((5, 5), np.uint8)
        blue_mask_binary = cv2.morphologyEx(binary, cv2.MORPH_OPEN, kernel)

        contours, hierarchy = cv2.findContours(blue_mask_binary, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

        if len(contours) > 0:
            boxes = [cv2.boundingRect(c) for c in contours]
            for box in boxes:
                x, y, w, h = box
                if w > 240: #加入蓝色区域面积限制，用于蓝色线的筛选
                    cv2.rectangle(blue_mask_binary, (x, y), (x + w, y + h), (153, 153, 0), 1)
                    for i in range(1, 639, 1):
                        if 255 in blue_mask_binary[370:389, i]:
                            blue_lane_score = blue_lane_score + 1
                            print("stop",stop)
                            if blue_lane_score > 2:
                                stop = 1
                                blue_lane_score = 0
                                print("stop",stop)
                                break
                            else:
                                stop = 0
                # print("blue_lane_score",blue_lane_score)
        cv2.rectangle(blue_mask_binary, (1, 400), (639, 439), (153, 153, 0), 2)
        cv2.imshow('find_blue_line_image', blue_mask_binary)
        cv2.waitKey(1)
        # dianshu = np.sum(blue_mask_binary[350:369,270:370])
        # print("dianshu",dianshu)
        return stop


# 最新蓝线校正
def blue_line_jiaozhun_final(img):
    global blue_line_heng_detect  # y=350
    global blue_line_shu_detect  # x=320
    global jiaozhun_angle_sum
    global jiaozhun_shuju_ave_index
    global angle_error_last
    blue_first_point_x = 0
    blue_first_point_y = 0
    blue_second_point_x = 0
    blue_second_point_y = 0
    speed_error = 0
    angle_error = 0
    blue_jiaozhun_stop = 0
    global p1_last
    global p2_last

    # 端点的检测
    p1, p2, line = blue_line.find_blue_line_give_points(img)

    # 图线的绘制
    line_img = blue_line.draw_line_2(img, line)
    cv2.imshow('draw_line:', line_img)
    cv2.waitKey(1)

    # 判断是否检测到新的蓝线1
    if p1 == None:
        print("未检测到第一个蓝色点")
        blue_first_point_x_miss = 1
    else:
        blue_first_point_x_miss = 0
        p1 = p1
    # 判断是否检测到新的蓝线2
    if p2 == None:
        print("未检测到第二个蓝色点")
        blue_second_point_x_miss = 1
    else:
        blue_second_point_x_miss = 0
        p2 = p2

    # 看看是否有miss的出现
    if blue_first_point_x_miss == 1 or blue_second_point_x_miss == 1:
        angle_error = None
        speed_error_fuhao = 0

    else:
        # 如果没有miss，就进行直线角度的计算
        vector = (p2[0] - p1[0], p2[1] - p1[1])
        angle_radians = np.arctan2(vector[1], vector[0])
        angle_degrees = np.degrees(angle_radians)
        angle_error = -angle_degrees
        print("jiaodu_wucha", angle_error)

        # 也进行直线中点的计算
        mid_point_y = (p1[1] + p2[1]) / 2
        # 使用375来减掉中点
        # 应该称为位置误差
        speed_error_fuhao = blue_line_heng_detect - mid_point_y
        print("location_wucha:", speed_error_fuhao)



    # 极性判断
    # 如果没有角度，就不动
    if angle_error == None:
        print("Can't find blue line ,return back")
        angle_error = 0
        speed_error = 0


    # 如果有角度，可以进行矫正
    else:
        if (0 <= angle_error < 3) or (0 > angle_error > -3):
            if -10 <= speed_error_fuhao <= 10:  # 校正完美
                speed_error = 0
                blue_jiaozhun_stop = 1
                print("perfect")
            elif speed_error_fuhao < -10:  # just后退
                speed_error = 15 * speed_dianji_fuhao
                print("just后退")
            elif speed_error_fuhao > 10:  # just前进
                speed_error = -15 * speed_dianji_fuhao
                print("just前进")


        elif angle_error < 90 and angle_error >= 3:
            if speed_error_fuhao < -10:  # 右转后退
                angle_error = 10
                speed_error = 15 * speed_dianji_fuhao
                print("右转后退")
            elif speed_error_fuhao > 10:  # 左转前进
                angle_error = -10
                speed_error = -15 * speed_dianji_fuhao
                print("左转前进")
            else:  # 右转后退+速度补偿
                angle_error = 0
                speed_error = 10 * speed_dianji_fuhao
                print("后退+速度补偿")

        elif -3 >= angle_error > -90:
            if speed_error_fuhao < -10:  # 左转后退
                angle_error = -10
                speed_error = 15 * speed_dianji_fuhao
                print("左转后退")
            elif speed_error_fuhao > 10:  # 右转前进
                angle_error = 10
                speed_error = -15 * speed_dianji_fuhao
                print("右转前进")
            else:
                angle_error = 0
                speed_error = 10 * speed_dianji_fuhao
                print("后退+速度补偿")

    # 一阶滞后滤波
    # 先去掉这个滤波
    angle_error_now = angle_error * 0.7 + angle_error_last * 0.3
    angle_error_last = angle_error_now

    p1_last = p1
    p2_last = p2
    return speed_error, angle_error_now, blue_jiaozhun_stop
